#include "Gps.h"
#include "TinyGPS.h"
#include <boost/thread/xtime.hpp>
#include <boost/function.hpp>
#include <iomanip>


Gps::Gps(short ComNum, int BaudRate):
SerialWrapper(ComNum, BaudRate),
m_shallStop(false),
m_Thread(NULL)
{

}

Gps::~Gps()
{
	this->Stop();
}

void Gps::WriteToBus(char* Message, int MessageSize)
{
	if(false == m_HandleOpened)
		return;

	std::cout << "Serial " << m_ComNum << " Write : ";
	for(int i = 0; i < MessageSize; i++)
		std::cout << std::setw(2) << std::hex << std::setfill('0') << static_cast<int>(*(Message + i)) << " ";

	std::cout << std::endl;
	write(m_Handle, Message, MessageSize);
}

void Gps::Run()
{
	m_shallStop = false;
	boost::function<void (void)>Func = boost::bind(&Gps::IterationsThread, this);
	m_Thread = new boost::thread(Func);
}

void Gps::IterationsThread()
{
	TinyGPS GPS;
	bool firstTime = true;
	while(false == m_shallStop)
	{
		if(true == m_HandleOpened)
		{
			if(true == firstTime)
			{
				char msg[] = {'$','P','M','T','K','3','1','4',',','0',',','1',',','0',',','1',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0',',','0','*','2','8',0x0D,0x0A};
				WriteToBus(msg, 51);
				firstTime = false;
			}
			/*char buffer[255];
			if(0 < read(m_Handle, buffer, 255))
				std::cout << buffer;*/
			char buff;
			while( 0 < read(m_Handle, &buff, 1))
			{
				std::cout << buff;
				if(true == GPS.encode(buff))
				{
					float flat, flon;
					unsigned long fix_age;

					// returns +/- latitude/longitude in degrees
					GPS.f_get_position(&flat, &flon, &fix_age);
					float falt = GPS.f_altitude(); // +/- altitude in meters
					float fc = GPS.f_course(); // course in degrees
					float fmps = GPS.f_speed_mps(); // speed in m/sec
					float fkmph = GPS.f_speed_kmph(); // speed in km/hr

					std::cout << flat << " " << flon << " " << falt << std::endl;
				}
			}
		}
		boost::this_thread::sleep(boost::posix_time::milliseconds(200));
	}
}

void Gps::Stop()
{
	m_shallStop = true;
	if(NULL != m_Thread)
	{
		m_Thread->join();
		delete m_Thread;
	}
	std::cout << "Gps Stopped!" << std::endl;
}
